#include "ImageUtility.h"

#include <QVector2D>
#include <QColor>
#include <QPainter>
#include <QDebug>
#include <qmath.h>

#ifndef M_E
#define M_E 2.71828182845904523536028747
#endif

ImageUtility::ImageUtility()
{
}

IplImage* ImageUtility::QImageToIplImage(QImage image)
{
    int channelCount = 3;

    image = image.convertToFormat(QImage::Format_RGB888);

    IplImage* iplImage = cvCreateImage(cvSize(image.width(), image.height()), IPL_DEPTH_8U, channelCount);

    uchar* newData = (uchar*)iplImage->imageData;

    memcpy(newData, image.bits(), image.byteCount());

    if (!image.isGrayscale()) {
        cvCvtColor(iplImage, iplImage, CV_RGB2BGR);
    }

    return iplImage;
}

QImage ImageUtility::clone(QImage image)
{
    QImage newImage(image.size(), image.format());
    memcpy(newImage.bits(), image.bits(), image.byteCount());

    return newImage;
}

QImage ImageUtility::IplImageToQImage(IplImage* image)
{
    IplImage* rgbFrame = cvCreateImage(cvGetSize(image), image->depth, 3);

    if (image->nChannels > 1) {
        cvCvtColor(image, rgbFrame, CV_BGR2RGB);
    } else {
        cvCvtColor(image, rgbFrame, CV_GRAY2RGB);
    }

    if (rgbFrame->depth != IPL_DEPTH_8U) {
        IplImage* newDepthImage = cvCreateImage(cvGetSize(rgbFrame), IPL_DEPTH_8U, rgbFrame->nChannels);

        cvCvtScale(rgbFrame, newDepthImage);

        cvReleaseImage(&rgbFrame);

        rgbFrame = newDepthImage;
    }


    QImage qImage(rgbFrame->width, rgbFrame->height, QImage::Format_RGB888);

    const uchar* bitsPtr = qImage.bits();

    memcpy((void*)bitsPtr, (const void*)rgbFrame->imageData, rgbFrame->imageSize);

    cvReleaseImage(&rgbFrame);

    return qImage;
}

QImage ImageUtility::convertFromLogRGB(QImage image)
{
    QImage newImage(image.size(), image.format());

    float maxLog = log(256.0);

    for (int r = 0; r < newImage.height(); r++) {
        for (int c = 0; c < newImage.width(); c++) {
            QColor pixel = image.pixel(c, r);

            float logRed = pixel.redF() * maxLog;
            float logGreen = pixel.greenF() * maxLog;
            float logBlue = pixel.blueF() * maxLog;

            int red = qRound(powf(M_E, logRed)) - 1;
            int green = qRound(powf(M_E, logGreen)) - 1;
            int blue = qRound(powf(M_E, logBlue)) - 1;

            pixel.setRed(red);
            pixel.setGreen(green);
            pixel.setBlue(blue);

            newImage.setPixel(c, r, pixel.rgb());
        }
    }

    return newImage;
}

QImage ImageUtility::convertToLogRGB(QImage image)
{
    QImage logImage(image.size(), image.format());

    float maxLog = log(256.0);

    for (int r = 0; r < logImage.height(); r++) {
        for (int c = 0; c < logImage.width(); c++) {
            QColor pixel = image.pixel(c, r);

            float logRed = log(pixel.red() + 1.0);
            float logGreen = log(pixel.green() + 1.0);
            float logBlue = log(pixel.blue() + 1.0);

            pixel.setRedF(logRed/maxLog);
            pixel.setGreenF(logGreen/maxLog);
            pixel.setBlueF(logBlue/maxLog);

            logImage.setPixel(c, r, pixel.rgb());
        }
    }

    return logImage;
}

QImage ImageUtility::convertToGrayscale(QImage image, enum Channel channel)
{
    QImage grayscale(image.size(), QImage::Format_RGB888);
    for (int r = 0; r < image.height(); r++) {
        for (int c = 0; c < image.width(); c++) {
            QColor pixel = image.pixel(c, r);
            float grayValue = 0.0f;
            if (channel == RED_CHANNEL) {
                grayValue = pixel.redF();
            } else if (channel == GREEN_CHANNEL) {
                grayValue = pixel.greenF();
            } else if (channel == BLUE_CHANNEL) {
                grayValue = pixel.blueF();
            } else if (channel == HSV_HUE_CHANNEL) {
                grayValue = pixel.hsvHueF();
            } else if (channel == HSV_SATURATION_CHANNEL) {
                grayValue = pixel.hsvSaturationF();
            } else if (channel == HSV_VALUE_CHANNEL) {
                grayValue = pixel.valueF();
            } else {
                grayValue = pixel.redF() * 0.3f + pixel.greenF() * 0.59f + pixel.blueF() * 0.11f;
            }
            QColor gray;
            gray.setRgbF(grayValue, grayValue, grayValue);
            grayscale.setPixel(c, r, gray.rgb());
        }
    }
    return grayscale;
}

QImage ImageUtility::add(QImage first, QImage second)
{
    QImage result(first.size(), first.format());

    for (int r = 0; r < result.height(); r++) {
        for (int c = 0; c < result.width(); c++) {
            QColor firstPixel = first.pixel(c, r);
            QColor secondPixel = second.pixel(c, r);
            QColor resultPixel;
            float red = firstPixel.redF() + secondPixel.redF();
            float green = firstPixel.greenF() + secondPixel.greenF();
            float blue = firstPixel.blueF() + secondPixel.blueF();
            if (red > 1) {
                red = 1.0f;
            }

            if (green > 1)  {
                green = 1.0f;
            }

            if (blue > 1) {
                blue = 1.0f;
            }
            resultPixel.setRgbF(red, green, blue);
            result.setPixel(c, r, resultPixel.rgb());
        }
    }

    return result;
}

QImage ImageUtility::subtract(QImage first, QImage second)
{
    QImage result(first.size(), first.format());

    for (int r = 0; r < result.height(); r++) {
        for (int c = 0; c < result.width(); c++) {
            QColor firstPixel = first.pixel(c, r);
            QColor secondPixel = second.pixel(c, r);
            QColor resultPixel;
            float red = firstPixel.redF() - secondPixel.redF();
            float green = firstPixel.greenF() - secondPixel.greenF();
            float blue = firstPixel.blueF() - secondPixel.blueF();
            if (red < 0) {
                red = 0.0f;
            }

            if (green < 0)  {
                green = 0.0f;
            }

            if (blue < 0) {
                blue = 0.0f;
            }
            resultPixel.setRgbF(red, green, blue);
            result.setPixel(c, r, resultPixel.rgb());
        }
    }

    return result;
}


float ImageUtility::minValue(QImage image, Channel channel, QImage mask)
{
    float minValue = std::numeric_limits<float>::max();

    for (int r = 0; r < image.height(); r++) {
        for (int c = 0; c < image.width(); c++) {
            if (!mask.isNull()) {
                QColor maskPixel = mask.pixel(c, r);
                if (maskPixel != Qt::white)
                    continue;
            }
            QColor pixel = image.pixel(c, r);
            float value = 0.0f;
            if (channel == RED_CHANNEL) {
                value = pixel.redF();
            } else if (channel == GREEN_CHANNEL) {
                value = pixel.greenF();
            } else if (channel == BLUE_CHANNEL) {
                value = pixel.blueF();
            } else if (channel == HSV_HUE_CHANNEL) {
                value = pixel.hsvHueF();
            } else if (channel == HSV_SATURATION_CHANNEL) {
                value = pixel.hsvSaturationF();
            } else if (channel == HSV_VALUE_CHANNEL) {
                value = pixel.valueF();
            } else {
                value = pixel.redF() * 0.3f + pixel.greenF() * 0.59f + pixel.blueF() * 0.11f;
            }
            if (value < minValue)
                minValue = value;
        }
    }

    return minValue;
}


float ImageUtility::maxValue(QImage image, Channel channel, QImage mask)
{
    float maxValue = std::numeric_limits<float>::min();

    for (int r = 0; r < image.height(); r++) {
        for (int c = 0; c < image.width(); c++) {
            if (!mask.isNull()) {
                QColor maskPixel = mask.pixel(c, r);
                if (maskPixel != Qt::white)
                    continue;
            }
            QColor pixel = image.pixel(c, r);
            float value = 0.0f;
            if (channel == RED_CHANNEL) {
                value = pixel.redF();
            } else if (channel == GREEN_CHANNEL) {
                value = pixel.greenF();
            } else if (channel == BLUE_CHANNEL) {
                value = pixel.blueF();
            } else if (channel == HSV_HUE_CHANNEL) {
                value = pixel.hsvHueF();
            } else if (channel == HSV_SATURATION_CHANNEL) {
                value = pixel.hsvSaturationF();
            } else if (channel == HSV_VALUE_CHANNEL) {
                value = pixel.valueF();
            } else {
                value = pixel.redF() * 0.3f + pixel.greenF() * 0.59f + pixel.blueF() * 0.11f;
            }
            if (value > maxValue)
                maxValue = value;
        }
    }

    return maxValue;
}

QVector<QVector<int> > ImageUtility::histogram(QImage image, QImage mask)
{
    QVector<int> red = ImageUtility::histogram(image, RED_CHANNEL, mask);
    QVector<int> green = ImageUtility::histogram(image, GREEN_CHANNEL, mask);
    QVector<int> blue = ImageUtility::histogram(image, BLUE_CHANNEL, mask);

    QVector<QVector<int> > colors;
    colors.append(red);
    colors.append(green);
    colors.append(blue);

    return colors;
}

QVector<int> ImageUtility::histogram(QImage image, Channel channel, QImage mask)
{
    QVector<int> histogram;
    histogram.fill(0, 256);

    for (int r = 0; r < image.height(); r++) {
        for (int c = 0; c < image.width(); c++) {
            if (!mask.isNull()) {
                QColor maskPixel = mask.pixel(c, r);
                if (maskPixel != Qt::white)
                    continue;
            }
            QColor pixel = image.pixel(c, r);
            int index = 0;
            if (channel == RED_CHANNEL) {
                index = pixel.red();
            } else if (channel == GREEN_CHANNEL) {
                index = pixel.green();
            } else if (channel == BLUE_CHANNEL) {
                index = pixel.blue();
            } else if (channel == HSV_HUE_CHANNEL) {
                index = pixel.hsvHueF() * 255;
            } else if (channel == HSV_SATURATION_CHANNEL) {
                index = pixel.hsvSaturationF() * 255;
            } else if (channel == HSV_VALUE_CHANNEL) {
                index = pixel.valueF() * 255;
            } else {
                index = pixel.red() * 0.3f + pixel.green() * 0.59f + pixel.blue() * 0.11f;
            }
            histogram[index]++;
        }
    }

    return histogram;
}

QImage ImageUtility::equalizeHistogram(QImage image, QImage mask)
{
    QVector<float> red = ImageUtility::equalizedHistogramLookupTable(image, RED_CHANNEL, mask);
    QVector<float> green = ImageUtility::equalizedHistogramLookupTable(image, GREEN_CHANNEL, mask);
    QVector<float> blue = ImageUtility::equalizedHistogramLookupTable(image, BLUE_CHANNEL, mask);

    QImage newImage = ImageUtility::clone(image);

    for (int r = 0; r < image.height(); r++) {
        for (int c = 0; c < image.width(); c++) {
            if (!mask.isNull()) {
                QColor maskPixel = mask.pixel(c, r);
                if (maskPixel != Qt::white)
                    continue;
            }
            QColor pixel = image.pixel(c, r);
            pixel.setRgbF(red[pixel.red()], green[pixel.green()], blue[pixel.blue()]);
            newImage.setPixel(c, r, pixel.rgb());
        }
    }

    return newImage;
}

QImage ImageUtility::equalizeHistogram(QImage image, Channel channel, QImage mask)
{
    QImage newImage = ImageUtility::clone(image);

    QVector<float> lookupTable = ImageUtility::equalizedHistogramLookupTable(image, channel, mask);

    for (int r = 0; r < image.height(); r++) {
        for (int c = 0; c < image.width(); c++) {
            if (!mask.isNull()) {
                QColor maskPixel = mask.pixel(c, r);
                if (maskPixel != Qt::white)
                    continue;
            }
            QColor pixel = image.pixel(c, r);
            int index = 0;
            if (channel == RED_CHANNEL) {
                index = pixel.red();
                pixel.setRedF(lookupTable[index]);
            } else if (channel == GREEN_CHANNEL) {
                index = pixel.green();
                pixel.setGreenF(lookupTable[index]);
            } else if (channel == BLUE_CHANNEL) {
                index = pixel.blue();
                pixel.setBlueF(lookupTable[index]);
            } else if (channel == HSV_HUE_CHANNEL) {
                index = pixel.hsvHueF() * 255;
                pixel.setHsvF(lookupTable[index], pixel.hsvSaturationF(), pixel.valueF());
            } else if (channel == HSV_SATURATION_CHANNEL) {
                index = pixel.hsvSaturationF() * 255;
                pixel.setHsvF(pixel.hsvHueF(), lookupTable[index], pixel.valueF());
            } else if (channel == HSV_VALUE_CHANNEL) {
                index = pixel.valueF() * 255;
                pixel.setHsvF(pixel.hsvHueF(), pixel.hsvSaturationF(), lookupTable[index]);
            }

            newImage.setPixel(c, r, pixel.rgb());
        }
    }

    return newImage;
}

QVector<float> ImageUtility::equalizedHistogramLookupTable(QImage image, Channel channel, QImage mask)
{
    QVector<int> histogram = ImageUtility::histogram(image, channel, mask);

    QVector<float> lookupTable;
    lookupTable.fill(0.0f, histogram.count());

    float imageArea = image.width() * image.height();

    if (!mask.isNull()) {
        int maskPixels = 0;
        for (int r = 0; r < mask.height(); r++) {
            for (int c = 0; c < mask.width(); c++) {
                QColor pixel = mask.pixel(c, r);
                if (pixel == Qt::white)
                    maskPixels++;
            }
        }
        imageArea = maskPixels;
    }

    int total = 0;
    for (int i = 0; i < histogram.count(); i++) {
        total += histogram[i];
        lookupTable[i] = (total / imageArea);
    }

    return lookupTable;
}

QVector<float> ImageUtility::equalizedHistogramInverseLookupTable(QImage image, Channel channel, QImage mask)
{
    QVector<float> lookupTable = ImageUtility::equalizedHistogramLookupTable(image, channel, mask);

    for (int i = 0; i < lookupTable.count(); i++) {
        lookupTable[i] *= 255;
    }

    QVector<float> inverseLookupTable;
    inverseLookupTable.fill(0.0f, 256);

    for (int i = 0; i < inverseLookupTable.count(); i++) {

        if (inverseLookupTable[i] != 0.0f) {
            continue;
        }

        float value = i;
        float mappedValue = 0.0f;
        for (int j = 1; j < lookupTable.count(); j++) {
            if (lookupTable[j] == value ||
                (lookupTable[j-1] <= value && lookupTable[j] >= value)) {
                mappedValue = j;
                break;
            }
        }

        inverseLookupTable[i] = mappedValue / 255.0f;
    }

    return inverseLookupTable;
}

QVector<int> ImageUtility::equalizedHistogram(QImage image, Channel channel, QImage mask)
{
    QImage equalized = ImageUtility::equalizeHistogram(image, channel, mask);

    QVector<int> histogram = ImageUtility::histogram(equalized, channel, mask);

    return histogram;
}

QVector<float> ImageUtility::specifiedHistogram(QImage source, QImage destination,
                                                Channel sourceChannel, Channel destinationChannel,
                                                QImage sourceMask, QImage destinationMask)
{
    QVector<float> equalizedLookupTable = ImageUtility::equalizedHistogramLookupTable(source, sourceChannel, sourceMask);

    QVector<float> inverse = ImageUtility::equalizedHistogramInverseLookupTable(destination, destinationChannel, destinationMask);

    QVector<float> mapping;
    mapping.fill(0.0f, 256);

    for (int i = 0; i < equalizedLookupTable.count(); i++) {
        float value = equalizedLookupTable[i];
        if (mapping[i] != 0)
            continue;
        float mappedValue = inverse[(int)qRound(value * 255)];
        mapping[i] = mappedValue;
    }

    return mapping;
}

QImage ImageUtility::specifyHistogram(QImage source, QImage destination, QImage sourceMask, QImage destinationMask)
{
    QVector<float> red = ImageUtility::specifiedHistogram(source, destination, RED_CHANNEL, RED_CHANNEL, sourceMask, destinationMask);
    QVector<float> green = ImageUtility::specifiedHistogram(source, destination, GREEN_CHANNEL, GREEN_CHANNEL, sourceMask, destinationMask);
    QVector<float> blue = ImageUtility::specifiedHistogram(source, destination, BLUE_CHANNEL, BLUE_CHANNEL, sourceMask, destinationMask);

    QImage newImage = ImageUtility::clone(source);

    for (int r = 0; r < source.height(); r++) {
        for (int c = 0; c < source.width(); c++) {
            if (!sourceMask.isNull()) {
                QColor maskPixel = sourceMask.pixel(c, r);
                if (maskPixel != Qt::white)
                    continue;
            }
            QColor pixel = source.pixel(c, r);
            pixel.setRgbF(red[pixel.red()], green[pixel.green()], blue[pixel.blue()]);
            newImage.setPixel(c, r, pixel.rgb());
        }
    }

    return newImage;
}

QImage ImageUtility::specifyHistogram(QImage source, QImage destination,
                                      Channel sourceChannel, Channel destinationChannel,
                                      QImage sourceMask, QImage destinationMask)
{

    QImage newImage = ImageUtility::clone(source);
    // Get a flat histogram for the first

    QVector<float> mapping = ImageUtility::specifiedHistogram(source, destination, sourceChannel, destinationChannel, sourceMask, destinationMask);

    for (int r = 0; r < newImage.height(); r++) {
        for (int c = 0; c < newImage.width(); c++) {
            if (!sourceMask.isNull()) {
                QColor maskPixel = sourceMask.pixel(c, r);
                if (maskPixel != Qt::white)
                    continue;
            }
            QColor pixel = newImage.pixel(c, r);
            int index = 0;
            if (sourceChannel == RED_CHANNEL) {
                index = pixel.red();
                pixel.setRedF(mapping[index]);
            } else if (sourceChannel == GREEN_CHANNEL) {
                index = pixel.green();
                pixel.setGreenF(mapping[index]);
            } else if (sourceChannel == BLUE_CHANNEL) {
                index = pixel.blue();
                pixel.setBlueF(mapping[index]);
            } else if (sourceChannel == HSV_HUE_CHANNEL) {
                index = pixel.hsvHueF() * 255;
                pixel.setHsvF(mapping[index], pixel.hsvSaturationF(), pixel.valueF());
            } else if (sourceChannel == HSV_SATURATION_CHANNEL) {
                index = pixel.hsvSaturationF() * 255;
                pixel.setHsvF(pixel.hsvHueF(), mapping[index], pixel.valueF());
            } else if (sourceChannel == HSV_VALUE_CHANNEL) {
                index = pixel.valueF() * 255;
                pixel.setHsvF(pixel.hsvHueF(), pixel.hsvSaturationF(), mapping[index]);
            }

            newImage.setPixel(c, r, pixel.rgb());
        }
    }

    return newImage;

}

QVector<float> ImageUtility::normalizedHistogram(QImage image, Channel channel, QImage mask)
{
    QVector<int> histogram = ImageUtility::histogram(image, channel, mask);
    float total;
    for (int i = 0; i < histogram.count(); i++) {
        total += histogram[i];
    }

    QVector<float> normalized;
    normalized.fill(0.0f, histogram.count());

    for (int i = 0; i < histogram.count(); i++) {
        normalized[i] = histogram[i] / total;
    }

    return normalized;
}

QVector<QVector<float> > ImageUtility::normalizedHistogram(QImage image, QImage mask)
{
    QVector<QVector<float> > normalized;

    normalized.append(ImageUtility::normalizedHistogram(image, RED_CHANNEL, mask));
    normalized.append(ImageUtility::normalizedHistogram(image, GREEN_CHANNEL, mask));
    normalized.append(ImageUtility::normalizedHistogram(image, BLUE_CHANNEL, mask));

    return normalized;
}

QVector<QImage> ImageUtility::gradientXY(QImage image)
{
    IplImage* ipl = ImageUtility::QImageToIplImage(image);

    IplImage* sobelX = cvCreateImage(cvSize(ipl->width, ipl->height), IPL_DEPTH_32F, ipl->nChannels);

    cvSobel(ipl, sobelX, 1, 0);

    ImageUtility::normalize(sobelX);

    IplImage* sobelY = cvCreateImage(cvSize(ipl->width, ipl->height), IPL_DEPTH_32F, ipl->nChannels);

    cvSobel(ipl, sobelY, 0, 1);

    ImageUtility::normalize(sobelY);

    IplImage* gradientX = cvCreateImage(cvSize(ipl->width, ipl->height), IPL_DEPTH_8U, ipl->nChannels);
    IplImage* gradientY = cvCreateImage(cvSize(ipl->width, ipl->height), IPL_DEPTH_8U, ipl->nChannels);

    for (int r = 0; r < ipl->height; r++) {
        for (int c = 0; c < ipl->width; c++) {
            CvScalar Ix = cvGet2D(sobelX, r, c);
            CvScalar Iy = cvGet2D(sobelY, r, c);

            QVector2D redGradient = QVector2D(Ix.val[0], Iy.val[0]);
            double redX;
            double redY;
            if (redGradient.length() == 0) {
                redX = 0;
                redY = 255;
            }
            redX = Ix.val[0]/redGradient.length() * 255;
            redY = Iy.val[0]/redGradient.length() * 255;

            if (ipl->nChannels > 1) {
                QVector2D greenGradient = QVector2D(Ix.val[1], Iy.val[1]);
                double greenX;
                double greenY;
                if (greenGradient.length() == 0) {
                    greenX = 0;
                    greenY = 255;
                }
                greenX = Ix.val[0]/greenGradient.length() * 255;
                greenY = Iy.val[0]/greenGradient.length() * 255;

                QVector2D blueGradient = QVector2D(Ix.val[2], Iy.val[2]);
                double blueX;
                double blueY;
                if (blueGradient.length() == 0) {
                    blueX = 0;
                    blueY = 255;
                }
                blueX = Ix.val[0]/blueGradient.length() * 255;
                blueY = Iy.val[0]/blueGradient.length() * 255;
            }

            cvSet2D(gradientX, r, c, Ix);
            cvSet2D(gradientY, r, c, Iy);
        }
    }

    cvReleaseImage(&sobelX);
    cvReleaseImage(&sobelY);

    cvReleaseImage(&ipl);

    QVector<QImage> images;
    QImage gradX = ImageUtility::IplImageToQImage(gradientX);
    QImage gradY = ImageUtility::IplImageToQImage(gradientX);
    images.append(gradX);
    images.append(gradY);
    cvReleaseImage(&gradientX);
    cvReleaseImage(&gradientY);

    return images;
}

QImage ImageUtility::gradientOrientation(QImage image)
{
    IplImage* ipl = ImageUtility::QImageToIplImage(image);

    IplImage* sobelX = cvCreateImage(cvSize(ipl->width, ipl->height), IPL_DEPTH_32F, ipl->nChannels);

    cvSobel(ipl, sobelX, 1, 0);

    IplImage* sobelY = cvCreateImage(cvSize(ipl->width, ipl->height), IPL_DEPTH_32F, ipl->nChannels);

    cvSobel(ipl, sobelY, 0, 1);

    IplImage* gradientOrientation = cvCreateImage(cvSize(ipl->width, ipl->height), IPL_DEPTH_8U, ipl->nChannels);

    for (int r = 0; r < ipl->height; r++) {
        for (int c = 0; c < ipl->width; c++) {
            CvScalar Ix = cvGet2D(sobelX, r, c);
            CvScalar Iy = cvGet2D(sobelY, r, c);

            double redGradient = atan2(Iy.val[0], Ix.val[0]);

            if (ipl->nChannels > 1) {
                double greenGradient = atan2(Iy.val[1], Ix.val[1]);
                double blueGradient = atan2(Iy.val[2], Ix.val[2]);



                Ix.val[1] = (greenGradient + M_PI) * (255/(2 * M_PI));
                Ix.val[2] = (blueGradient + M_PI) * (255/(2 * M_PI));
            }

            Ix.val[0] = (redGradient + M_PI) * (255/(2 * M_PI));

            cvSet2D(gradientOrientation, r, c, Ix);
        }
    }

    cvReleaseImage(&sobelX);
    cvReleaseImage(&sobelY);

    cvReleaseImage(&ipl);

    QImage gradOrientation = ImageUtility::IplImageToQImage(gradientOrientation);
    cvReleaseImage(&gradientOrientation);

    return gradOrientation;
}

QImage ImageUtility::gradientMagnitude(QImage image)
{
    IplImage* ipl = ImageUtility::QImageToIplImage(image);

    IplImage* sobelX = cvCreateImage(cvSize(ipl->width, ipl->height), IPL_DEPTH_32F, ipl->nChannels);

    cvSobel(ipl, sobelX, 1, 0);

    //ImageUtility::normalize(sobelX);

    IplImage* sobelY = cvCreateImage(cvSize(ipl->width, ipl->height), IPL_DEPTH_32F, ipl->nChannels);

    cvSobel(ipl, sobelY, 0, 1);

    //ImageUtility::normalize(sobelY);

    IplImage* gradientMagnitude = cvCreateImage(cvSize(ipl->width, ipl->height), IPL_DEPTH_8U, ipl->nChannels);

    for (int r = 0; r < ipl->height; r++) {
        for (int c = 0; c < ipl->width; c++) {
            CvScalar Ix = cvGet2D(sobelX, r, c);
            CvScalar Iy = cvGet2D(sobelY, r, c);

            QVector2D redGradient = QVector2D(Ix.val[0], Iy.val[0]);

            double redGM = redGradient.length();

            if (ipl->nChannels > 1) {
                QVector2D greenGradient = QVector2D(Ix.val[1], Iy.val[1]);
                QVector2D blueGradient = QVector2D(Ix.val[2], Iy.val[2]);

                double greenGM = greenGradient.length();
                double blueGM = blueGradient.length();


                Ix.val[1] = greenGM;
                Ix.val[2] = blueGM;
            }

            Ix.val[0] = redGM;

            cvSet2D(gradientMagnitude, r, c, Ix);
        }
    }

    cvReleaseImage(&sobelX);
    cvReleaseImage(&sobelY);

    cvReleaseImage(&ipl);

    QImage gradMag = ImageUtility::IplImageToQImage(gradientMagnitude);
    cvReleaseImage(&gradientMagnitude);

    return gradMag;
}

void ImageUtility::normalize(IplImage* image)
{
    int redMax = std::numeric_limits<int>::min();
    int greenMax = std::numeric_limits<int>::min();
    int blueMax = std::numeric_limits<int>::min();

    int redMin = std::numeric_limits<int>::max();
    int greenMin = std::numeric_limits<int>::max();
    int blueMin = std::numeric_limits<int>::max();

    for (int r = 0; r < image->height; r++) {
        for (int c = 0; c < image->width; c++) {
            CvScalar pixel = cvGet2D(image, r, c);

            if (pixel.val[0] > redMax) {
                redMax = pixel.val[0];
            }
            if (pixel.val[0] < redMin) {
                redMin = pixel.val[0];
            }

            if (image->nChannels > 1) {
                if (pixel.val[1] > greenMax) {
                    greenMax = pixel.val[1];
                }
                if (pixel.val[1] < greenMin) {
                    greenMin = pixel.val[1];
                }

                if (pixel.val[2] > blueMax) {
                    blueMax = pixel.val[2];
                }
                if (pixel.val[2] < blueMin) {
                    blueMin = pixel.val[2];
                }
            }
        }
    }

    float redRange = redMax - redMin;
    float greenRange = greenMax - greenMin;
    float blueRange = blueMax - blueMin;

    for (int r = 0; r < image->height; r++) {
        for (int c = 0; c < image->width; c++) {

            CvScalar pixel = cvGet2D(image, r, c);
            pixel.val[0] = (pixel.val[0] - redMin) / redRange * 255;
            if (image->nChannels > 1) {
                pixel.val[1] = (pixel.val[1] - greenMin) / greenRange * 255;
                pixel.val[2] = (pixel.val[2] - blueMin) / blueRange * 255;
            }
            cvSet2D(image, r, c, pixel);
        }
    }
}

QImage ImageUtility::normalize(QImage image)
{
    QImage newImage(image.size(), image.format());
    int redMax = std::numeric_limits<int>::min();
    int greenMax = std::numeric_limits<int>::min();
    int blueMax = std::numeric_limits<int>::min();

    int redMin = std::numeric_limits<int>::max();
    int greenMin = std::numeric_limits<int>::max();
    int blueMin = std::numeric_limits<int>::max();
    for (int r = 0; r < newImage.height(); r++) {
        for (int c = 0; c < newImage.width(); c++) {
            QColor pixel = image.pixel(c, r);
            if (pixel.red() > redMax) {
                redMax = pixel.red();
            }
            if (pixel.green() > greenMax) {
                greenMax = pixel.green();
            }
            if (pixel.blue() > blueMax) {
                blueMax = pixel.blue();
            }

            if (pixel.red() < redMin) {
                redMin = pixel.red();
            }
            if (pixel.green() < greenMin) {
                greenMin = pixel.green();
            }
            if (pixel.blue() < blueMin) {
                blueMin = pixel.blue();
            }
        }
    }

    float redRange = redMax - redMin;
    float greenRange = greenMax - greenMin;
    float blueRange = blueMax - blueMin;

    for (int r = 0; r < newImage.height(); r++) {
        for (int c = 0; c < newImage.width(); c++) {

            QColor pixel = image.pixel(c, r);
            pixel.setRgb((pixel.red() - redMin) / redRange * 255,
                         (pixel.green() - greenMin) / greenRange * 255,
                         (pixel.blue() - blueMin) / blueRange * 255);

            newImage.setPixel(c, r, pixel.rgb());
        }
    }
    return newImage;
}

QImage ImageUtility::differenceOfGaussians(QImage image, int firstKernelSize, int secondKernelSize)
{
    IplImage* ipl = ImageUtility::QImageToIplImage(image);
    IplImage* first = cvCreateImage(cvGetSize(ipl), ipl->depth, ipl->nChannels);
    IplImage* second = cvCreateImage(cvGetSize(ipl), ipl->depth, ipl->nChannels);

    cvSmooth(ipl, first, CV_GAUSSIAN, firstKernelSize);
    cvSmooth(ipl, second, CV_GAUSSIAN, secondKernelSize);

    cvSub(first, second, ipl);

    QImage newImage = ImageUtility::IplImageToQImage(ipl);

    newImage = ImageUtility::normalize(newImage);

    cvReleaseImage(&first);
    cvReleaseImage(&second);
    cvReleaseImage(&ipl);

    return newImage;
}

QImage ImageUtility::dilate(QImage image, int iterations)
{
    IplImage* ipl = ImageUtility::QImageToIplImage(image);

    IplImage* dilated = cvCreateImage(cvGetSize(ipl), ipl->depth, ipl->nChannels);

    cvDilate(ipl, dilated, 0, iterations);

    QImage newImage = ImageUtility::IplImageToQImage(dilated);

    cvReleaseImage(&ipl);
    cvReleaseImage(&dilated);

    return newImage;
}

QImage ImageUtility::smooth(QImage image, int kernelSize, ImageUtility::BlurType blurType)
{
    if (kernelSize % 2 == 0)
        kernelSize++;
    IplImage* ipl = ImageUtility::QImageToIplImage(image);

    IplImage* smooth = cvCreateImage(cvGetSize(ipl), ipl->depth, ipl->nChannels);

    cvSmooth(ipl, smooth, blurType, kernelSize, kernelSize, 30, 30);

    QImage newImage = ImageUtility::IplImageToQImage(smooth);

    cvReleaseImage(&ipl);

    cvReleaseImage(&smooth);

    return newImage;
}

QImage ImageUtility::erode(QImage image, int iterations)
{
    IplImage* ipl = ImageUtility::QImageToIplImage(image);

    IplImage* eroded = cvCreateImage(cvGetSize(ipl), ipl->depth, ipl->nChannels);

    cvErode(ipl, eroded, 0, iterations);

    QImage newImage = ImageUtility::IplImageToQImage(eroded);

    cvReleaseImage(&ipl);
    cvReleaseImage(&eroded);

    return newImage;
}

QImage ImageUtility::invert(QImage image)
{
    QImage inverted(image.size(), image.format());

    for (int r = 0; r < image.height(); r++) {
        for (int c = 0; c < image.width(); c++) {
            QColor pixel = image.pixel(c, r);
            inverted.setPixel(c, r, qRgb(255 - pixel.red(), 255 - pixel.green(), 255 - pixel.blue()));
        }
    }

    return inverted;
}

QImage ImageUtility::scale(QImage image, float scaleFactor)
{
    IplImage* ipl = ImageUtility::QImageToIplImage(image);

    IplImage* scaled = cvCreateImage(cvSize(ipl->width * scaleFactor, ipl->height * scaleFactor), ipl->depth, ipl->nChannels);

    cvResize(ipl, scaled);

    QImage newImage = ImageUtility::IplImageToQImage(scaled);

    cvReleaseImage(&ipl);

    cvReleaseImage(&scaled);

    return newImage;
}

QImage ImageUtility::fillHoles(QImage image, int pixelLength)
{
    QImage newImage(image.size(), image.format());
    memcpy(newImage.bits(), image.bits(), image.byteCount());
    QColor white(255, 255, 255);
    for (int r = 1; r < image.height() - pixelLength; r++) {
        for (int c = 1; c < image.width() - pixelLength; c++) {
            QColor pixel = image.pixel(c, r);
            if (pixel != QColor(0, 0, 0))
                continue;

            QColor leftPixel = image.pixel(c - 1, r);
            QColor rightPixel = image.pixel(c + pixelLength, r);
            int tempPixelLength = pixelLength;
            while (rightPixel != white && tempPixelLength > 0) {
                tempPixelLength--;
                rightPixel = image.pixel(c + tempPixelLength, r);
            }

            if (leftPixel == white && rightPixel == white) {
                for (int i = c; i < c + tempPixelLength; i++) {
                    newImage.setPixel(i, r, white.rgb());
                }
            }

            QColor topPixel = image.pixel(c, r - 1);
            QColor bottomPixel = image.pixel(c, r + pixelLength);
            tempPixelLength = pixelLength;
            while (bottomPixel != white && tempPixelLength > 0) {
                tempPixelLength--;
                bottomPixel = image.pixel(c, r + tempPixelLength);
            }

            if (topPixel == white && bottomPixel == white) {
                for (int i = r; i < r + tempPixelLength; i++) {
                    newImage.setPixel(c, i, white.rgb());
                }
            }
        }
    }

    return newImage;
}

QImage ImageUtility::changeColor(QImage image, QColor oldColor, QColor newColor)
{
    QImage newImage(image.size(), image.format());
    memcpy(newImage.bits(), image.bits(), image.byteCount());
    for (int r = 0; r < newImage.height(); r++) {
        for (int c = 0; c < newImage.width(); c++) {
            QColor pixel = newImage.pixel(c, r);
            if (pixel != oldColor)
                continue;
            newImage.setPixel(c, r, newColor.rgba());
        }
    }

    return newImage;
}

double ImageUtility::lineDistanceToPoint(QLine line, QPoint point)
{
    QPoint first = line.p1();
    QPoint second = line.p2();
    double a = first.y() - second.y();
    double b = -(first.x() - second.x());
    double c = first.x() * second.y() - first.y() * second.x();

    double normalizingDistanceFactor = (1/sqrt(a*a + b*b));

    return abs((a * point.x() + b * point.y() + c) * normalizingDistanceFactor);
}

QLine ImageUtility::findLineClosestToPoint(QVector<QLine>& lines, QPoint point)
{
    QLine minLine;
    double minDistanceToLine = std::numeric_limits<double>::max();
    foreach (QLine line, lines) {
        double distanceToLine = ImageUtility::lineDistanceToPoint(line, point);
        if (distanceToLine < minDistanceToLine) {
            minDistanceToLine = distanceToLine;
            minLine = line;
        }
    }

    return minLine;
}

double ImageUtility::findLineAngle(QLine line)
{
    return ImageUtility::findLineAngle(line.dx(), line.dy());
}

double ImageUtility::findLineAngle(QLineF line)
{
    return ImageUtility::findLineAngle(line.dx(), line.dy());
}

double ImageUtility::findLineAngle(double dx, double dy)
{
    double angle = 0;
    if (dx > 0) {
        angle = atan2(dy, dx);
    } else if (dx < 0 && dy >= 0) {
        angle = atan2(dy, dx) + M_PI;
    } else if (dx < 0 && dy < 0) {
        angle = atan2(dy, dx) - M_PI;
    } else if (dx == 0 && dy > 0) {
        angle = M_PI/2.0;
    } else if (dx == 0 && dy < 0) {
        angle = -M_PI/2.0;
    }

    return angle;
}

QLineF ImageUtility::findNormal(QLineF line)
{
    QLineF normal;

    QPointF middle = (line.p1() + line.p2())/2.0f;

    normal.setP1(QPointF(middle.x() - line.dy(), middle.y() + line.dx()));
    normal.setP2(middle);

    normal.translate(normal.dx()/2.0f, normal.dy()/2.0f);

    return normal;
}

QLine ImageUtility::findNormal(QLine line)
{
    QLineF normal = ImageUtility::findNormal(QLineF(line));

    return normal.toLine();
}

QLine ImageUtility::findLineThroughPoint(QLine line, QPoint point, float radius)
{
    QVector2D diff(line.x1() - line.x2(), line.y1() - line.y2());

    float scale = (radius * 2.0f) / diff.length();

    QLineF newLine;

    newLine.setP1(QPointF(point.x() - (line.dx()/2.0f) * scale, point.y() - (line.dy()/2.0f) * scale));
    newLine.setP2(QPointF(point.x() + (line.dx()/2.0f) * scale, point.y() + (line.dy()/2.0f) * scale));

    return newLine.toLine();
}

QVector<QLine> ImageUtility::findEdgeNormals(QImage image, float insidePixelWidth, float outsidePixelWidth)
{
    int kernelSize = image.width() > image.height() ? image.height() / 3 : image.width() / 3;
    QImage smoothed = ImageUtility::smooth(image, kernelSize, GAUSSIAN);
    QImage gradientMagnitude = ImageUtility::gradientMagnitude(image);
    gradientMagnitude.save("/home/joshabb/Desktop/dude.png");
    smoothed.save("/home/joshabb/Desktop/dude1.png");
    QImage gradientOrientation = ImageUtility::gradientOrientation(smoothed);

    QVector<QLine> lines;
    for (int r = 0; r < image.height(); r++) {
        for (int c = 0; c < image.width(); c++) {
            QColor edge = gradientMagnitude.pixel(c, r);
            if (edge != Qt::white)
                continue;

            QColor pixel = gradientOrientation.pixel(c, r);
            double angle = ((pixel.redF() - 0.5) * M_PI) / 0.5;
            int x1 = c - outsidePixelWidth * cos(angle);
            int y1 = r - outsidePixelWidth * sin(angle);
            int x2 = c + insidePixelWidth * cos(angle);
            int y2 = r + insidePixelWidth * sin(angle);

            QLine line(x1, y1, x2, y2);
            lines.append(line);

        }
    }

    return lines;
}

QLine ImageUtility::findLineInMask(QPoint point, int dx, int dy, QImage mask)
{
    double angle = ImageUtility::findLineAngle(dx, dy);
    double radius = 0;

    QPointF p1(point.x() - radius * cos(angle),
               point.y() - radius * sin(angle));
    QPointF p2(point.x() + radius * cos(angle),
               point.y() + radius * sin(angle));

    QColor pixel = mask.pixel(p1.toPoint());
    while (pixel == Qt::white) {
        radius++;
        p1 = QPointF(point.x() - radius * cos(angle),
                     point.y() - radius * sin(angle));
        pixel = mask.pixel(p1.toPoint());
    }
    radius = 0;
    pixel = mask.pixel(p2.toPoint());
    while (pixel == Qt::white) {
        radius++;
        p2 = QPointF(point.x() + radius * cos(angle),
                     point.y() + radius * sin(angle));
        pixel = mask.pixel(p2.toPoint());
    }

    QLineF line(p1, p2);

    return line.toLine();
}

QVector<QPoint> ImageUtility::getRasterizedPointsAlongLine(QLine line, QImage mask)
{
    QVector<QPoint> points;
    float dy = line.dy();
    float dx = line.dx();

    if (dy == 0.0f) {
        dx /= fabs(dx);
    } else if (dx == 0.0f) {
        dy /= fabs(dy);
    } else if (fabs(dy) > fabs(dx)) {
        dx /= (fabs(dy) + 1);
        dy /= fabs(dy);
    } else if (fabs(dx) > fabs(dy)) {
        dy /= (fabs(dx) + 1);
        dx /= fabs(dx);
    } else {
        dy /= fabs(dy);
        dx /= fabs(dx);
    }

    float r = line.y1();
    float c = line.x1();
    while ((dy > 0 ? r <= line.y2() : r >= line.y2()) &&
           (dx > 0 ? c <= line.x2() : c >= line.x2())) {
            int row = qRound(r);
            int col = qRound(c);
            r+=dy;
            c+=dx;
            if (!mask.isNull()) {
                if (row < 0 || col < 0 || row >= mask.height() || row >= mask.width())
                    continue;
                QColor pixel = mask.pixel(col, row);
                if (pixel == Qt::black)
                    continue;
            }
            points.append(QPoint(col, row));

    }

    return points;
}

QVector<QColor> ImageUtility::getImageValuesAlongLine(QImage image, QLine line)
{
    QVector<QColor> colors;
    QVector<int> red = ImageUtility::getImageValuesAlongLine(image, line, RED_CHANNEL);
    QVector<int> green = ImageUtility::getImageValuesAlongLine(image, line, GREEN_CHANNEL);
    QVector<int> blue = ImageUtility::getImageValuesAlongLine(image, line, BLUE_CHANNEL);

    for (int i = 0; i < red.count(); i++) {
        QColor color(red[i], green[i], blue[i]);
        colors.append(color);
    }

    return colors;
}

QVector<int> ImageUtility::getImageValuesAlongLine(QImage image, QLine line, enum Channel channel)
{
    QVector<int> values;
    float dy = line.dy();
    float dx = line.dx();

    if (dy == 0.0f) {
        dx /= fabs(dx);
    } else if (dx == 0.0f) {
        dy /= fabs(dy);
    } else if (fabs(dy) > fabs(dx)) {
        dx /= (fabs(dy) + 1);
        dy /= fabs(dy);
    } else if (fabs(dx) > fabs(dy)) {
        dy /= (fabs(dx) + 1);
        dx /= fabs(dx);
    } else {
        dy /= fabs(dy);
        dx /= fabs(dx);
    }

    float r = line.y1();
    float c = line.x1();
    while ((dy > 0 ? r <= line.y2() : r >= line.y2()) &&
           (dx > 0 ? c <= line.x2() : c >= line.x2())) {
            QColor pixel;
            int row = qRound(r);
            int col = qRound(c);
            if (row >= 0 && row <= image.height() - 1 &&
                col >= 0 && col <= image.width() - 1) {
                pixel = image.pixel(col, row);
            }

            int y;
            if (channel == RED_CHANNEL) {
                y = pixel.red();
            } else if (channel == GREEN_CHANNEL) {
                y = pixel.green();
            } else if (channel == BLUE_CHANNEL) {
                y = pixel.blue();
            } else if (channel == HSV_HUE_CHANNEL) {
                y = pixel.hsvHue();
            } else if (channel == HSV_SATURATION_CHANNEL) {
                y = pixel.hsvSaturation();
            } else if (channel == HSV_VALUE_CHANNEL) {
                y = pixel.value();
            } else {
                y = pixel.red() * 0.3f + pixel.green() * 0.59f + pixel.blue() * 0.11f;
            }

            values.append(y);

            r+=dy;
            c+=dx;
    }

    return values;
}

QImage ImageUtility::createLineImage(QVector<QLine> lines, QSize size, QVector<QColor> lineColors, QColor backgroundColor, QVector<float> lineWidths)
{
    if (lineColors.count() != lines.count()) {
        qDebug("Line colors count and lines count must match!");
        return QImage();
    }
    QImage image(size, QImage::Format_ARGB32);

    image.fill(backgroundColor.rgba());

    QPainter painter(&image);

    for (int i = 0; i < lines.count(); i++) {
        QLine line = lines[i];
        QColor color = lineColors[i];
        float width = lineWidths.count() > i ? lineWidths[i] : 1.0f;
        QPen pen(color);
        pen.setWidthF(width);
        painter.setPen(QPen(color));
        painter.drawLine(line);
    }

    return image;
}

QImage ImageUtility::createLineImage(QVector<QLine> lines, QSize size, QColor lineColor, QColor backgroundColor, float lineWidth)
{
    QImage image(size, QImage::Format_ARGB32);

    image.fill(backgroundColor.rgba());

    QPainter painter(&image);
    QPen pen(lineColor);
    pen.setWidthF(lineWidth);
    painter.setPen(pen);
    foreach (QLine line, lines) {
        painter.drawLine(line);
    }

    return image;
}
